#include "Map.h"


using namespace std;
using namespace cv;

Map::Map():
	m_MaxKeyFrameId(0),
	m_BigChangeIdx(0)
{

}

Map::~Map()
{

}

void Map::AddMapPoint(MapPoint* pMP)
{
	unique_lock<mutex> lock(m_MutexMap);
	m_setMapPoints.insert(pMP);
}


void Map::EraseMapPoint(MapPoint* pMP)
{
	unique_lock<mutex> lock(m_MutexMap);
	m_setMapPoints.erase(pMP);
}


void Map::AddKeyFrame(KeyFrame* pKF)
{
	unique_lock<mutex> lock(m_MutexMap);
	m_setKeyFrames.insert(pKF);

	if(pKF->m_KeyFrameId > m_MaxKeyFrameId)
	{
		m_MaxKeyFrameId = pKF->m_KeyFrameId;
	}
}


void Map::EraseKeyFrame(KeyFrame* pKF)
{
	unique_lock<mutex> lock(m_MutexMap);
	m_setKeyFrames.erase(pKF);
}


void Map::SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs)
{
	unique_lock<mutex> lock(m_MutexMap);
	m_vectorReferenceMapPoints = vpMPs;
}


void Map::InformNewBigChange()
{
	unique_lock<mutex> lock(m_MutexMap);
	m_BigChangeIdx++;
}


int Map::GetLastBigChangeIdx()
{
	unique_lock<mutex> lock(m_MutexMap);
	return m_BigChangeIdx;
}

std::vector<KeyFrame*> Map::GetAllKeyFrames()
{
	unique_lock<mutex> lock(m_MutexMap);
	return vector<KeyFrame*>(m_setKeyFrames.begin(),m_setKeyFrames.end());
}

vector<MapPoint*> Map::GetAllMapPoints()
{
	unique_lock<mutex> lock(m_MutexMap);
	return vector<MapPoint*>(m_setMapPoints.begin(),m_setMapPoints.end());
}


std::vector<MapPoint*> Map::GetReferenceMapPoints()
{
	unique_lock<mutex> lock(m_MutexMap);
	return m_vectorReferenceMapPoints;
}

long unsigned int Map::MapPointsSize()
{
	unique_lock<mutex> lock(m_MutexMap);
	return m_setMapPoints.size();
}


long unsigned  Map::KeyFramesSize()
{
	unique_lock<mutex> lock(m_MutexMap);
	return m_setKeyFrames.size();
}


long unsigned int Map::GetMaxKFid()
{
	unique_lock<mutex> lock(m_MutexMap);
	return m_MaxKeyFrameId;
}


void Map::clear()
{
	 for(set<MapPoint*>::iterator it=m_setMapPoints.begin(), end=m_setMapPoints.end(); it!=end; it++)
	 {
		delete *it;
	 }

	 for(set<KeyFrame*>::iterator it=m_setKeyFrames.begin(), end=m_setKeyFrames.end(); it!=end; it++)
	 {
		delete *it;
	 }

	  m_setMapPoints.clear();
	  m_setKeyFrames.clear();
	  m_MaxKeyFrameId = 0;

	  m_vectorReferenceMapPoints.clear();
	  m_vectorKeyFrameOrigins.clear();
}

